/*****************************************************************************
 * $Workfile:: curve.c                                                       $
 * $Revision:: 1                                                             $
 * $JustDate::  1/27/12                                                      $
 *
 *      GENIE V ECU
 *      main loop
 *****************************************************************************/

#include "main.h"
#include "curve.h"
#include "nvm.h"
#include "control.h"


struct curve drive_fast_curve;
struct curve drive_rised_curve;
struct curve lift_up_curve;
//struct curve steer_fast_curve;
struct curve drive_slow_curve;
struct curve steer_rised_curve;

static void CURVE_Copy(struct nvm_curve * pNvmCurve, struct curve *pCurve,
                       UINT8 bJoystickNeutralZone);

/*******************************************************************************
 *
 * PROCEDURE:  CURVE_IsEnabled( )
 *
 * DESCRIPTION: return a 0 if curve is diabled.
 *
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/

UINT8
CURVE_IsEnabled(struct curve * pc)
{
   // curve is stricly increasing  pice-wise function to joystick count.
   // the curve is disabled if curve is zeroed out.
   return pc->abPwmPos[ 5];
}

/*******************************************************************************
 *
 * PROCEDURE:  CURVE_GetPwm()
 *
 * DESCRIPTION: iprocess stricly increasing  pice-wise function to count PWM.
 *              The next limitation is concidered:
 *                      curve disigned with respect to the 1 count neutral zone.
 *                      thus the value of the neutral zone
 *                      expected to be substructed from actual
 *                      joystick count before processing
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/

UINT8
CURVE_GetPwm(INT16 nIn,
             struct curve * pc)
{
   UINT8 ix;
   UINT8 bY, bX;
   UINT8 bOffset;
   UINT16 wIn;

   if(nIn < 0)
   {
      wIn = 0 - nIn;
   }
   else
   {
      wIn = nIn;
   }

   for(ix = 1; ix < NUMBER_OF_BREAK_POINTS; ix++)
   {
      if(wIn <= pc->abIndex[ ix])
      {
         bX = pc->abIndex[ ix] - pc->abIndex[ ix - 1];
         wIn -= pc->abIndex[ ix - 1];

         if(nIn < 0)
         {
            bY = pc->abPwmNeg[ ix] - pc->abPwmNeg[ ix - 1];
            bOffset = pc->abPwmNeg[ ix - 1];
         }
         else
         {
            bY = pc->abPwmPos[ ix] - pc->abPwmPos[ ix - 1];
            bOffset = pc->abPwmPos[ ix - 1];
         }

         if(bX)
         {
            return (bOffset + (UINT8)(((UINT16)bY * (UINT16)wIn) / bX));
         }
         else
         {
            return (bOffset);
         }
      }
   }
   if(nIn < 0)
	    return pc->abPwmNeg[ NUMBER_OF_BREAK_POINTS - 1];
   else
	    return pc->abPwmPos[ NUMBER_OF_BREAK_POINTS - 1];
}

/*******************************************************************************
 *
 * PROCEDURE:  CURVE_Setup()
 *
 * DESCRIPTION: It adjust and copies the default curves.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/

void
CURVE_Setup(void)
{
   UINT8 bNeutralZone;

   bNeutralZone = lift_setup.bJoystickNeutralZone;

   CURVE_Copy(&lift_setup.drive_fast_curve,
              &drive_fast_curve,
              bNeutralZone);

   CURVE_Copy(&lift_setup.drive_rised_curve,
              &drive_rised_curve,
              bNeutralZone);

   CURVE_Copy(&lift_setup.lift_up_curve,
              &lift_up_curve,
              bNeutralZone);

   CURVE_Copy(&lift_setup.drive_slow_curve,
              &drive_slow_curve,
              bNeutralZone);

   //    CURVE_Copy( &lift_setup.steer_fast_curve,
   //                &steer_fast_curve,
   //                bNeutralZone);
   //
   CURVE_Copy(&lift_setup.steer_rised_curve,
              &steer_rised_curve,
              bNeutralZone);

}

/*******************************************************************************
 *
 * PROCEDURE:  CURVE_Setup()
 *
 * DESCRIPTION: It adjust the default curve.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/


static void
CURVE_Copy(struct nvm_curve * pNvmCurve,
           struct curve *pCurve,
           UINT8 bJoystickNeutralZone)
{
   UINT16 wAdjust;
   UINT8  bPwmMax;

#define PWMPOS_ADJUST(InDeX) \
    (((UINT16)pNvmCurve->bPwmPos ## InDeX * wAdjust ) / bPwmMax)
#define PWMNEG_ADJUST(InDeX) \
    (((UINT16)pNvmCurve->bPwmNeg ## InDeX * wAdjust ) / bPwmMax)

   // copy curve to memory and initialize pointer to setup
   pCurve->abIndex[ 0] = 0;
   pCurve->abIndex[ 1] = pNvmCurve->bIndex1;
   pCurve->abIndex[ 2] = pNvmCurve->bIndex2;
   pCurve->abIndex[ 3] = pNvmCurve->bIndex3;
   pCurve->abIndex[ 4] = pNvmCurve->bIndex4;
   pCurve->abIndex[ 5] = 127 - bJoystickNeutralZone;
   //
   // calculate adjusted curve.
   //
   wAdjust = pNvmCurve->bPwmPosAdjust;
   bPwmMax = pNvmCurve->bPwmPosMax;

   //
   // limit values
   //
   if(wAdjust > SMAL_SLAB_FAST_PWM_LIMIT)
   {
      wAdjust = SMAL_SLAB_FAST_PWM_LIMIT;
   }

   //
   //
   //
   if(bPwmMax)
   {
      pCurve->abPwmPos[ 5] = wAdjust;
      pCurve->abPwmPos[ 4] = PWMPOS_ADJUST(4);
      pCurve->abPwmPos[ 3] = PWMPOS_ADJUST(3);
      pCurve->abPwmPos[ 2] = PWMPOS_ADJUST(2);
      pCurve->abPwmPos[ 1] = PWMPOS_ADJUST(1);
      pCurve->abPwmPos[ 0] = 0;
   }
   else
   {
      pCurve->abPwmPos[ 5] = 0;
      pCurve->abPwmPos[ 4] = 0;
      pCurve->abPwmPos[ 3] = 0;
      pCurve->abPwmPos[ 2] = 0;
      pCurve->abPwmPos[ 1] = 0;
      pCurve->abPwmPos[ 0] = 0;
   }

   //
   // calculate adjusted curve.
   //
   //GTF    wAdjust = pNvmCurve->bPwmNegAdjust;
   wAdjust = pNvmCurve->bPwmPosAdjust;
   bPwmMax = pNvmCurve->bPwmNegMax;

   //
   // limit values
   //
   if(wAdjust > SMAL_SLAB_FAST_PWM_LIMIT)
   {
      wAdjust = SMAL_SLAB_FAST_PWM_LIMIT;
   }

   //
   //
   //
   if(bPwmMax)
   {
      pCurve->abPwmNeg[ 5] = wAdjust;
      pCurve->abPwmNeg[ 4] = PWMNEG_ADJUST(4);
      pCurve->abPwmNeg[ 3] = PWMNEG_ADJUST(3);
      pCurve->abPwmNeg[ 2] = PWMNEG_ADJUST(2);
      pCurve->abPwmNeg[ 1] = PWMNEG_ADJUST(1);
      pCurve->abPwmNeg[ 0] = 0;
   }
   else
   {
      pCurve->abPwmNeg[ 5] = 0;
      pCurve->abPwmNeg[ 4] = 0;
      pCurve->abPwmNeg[ 3] = 0;
      pCurve->abPwmNeg[ 2] = 0;
      pCurve->abPwmNeg[ 1] = 0;
      pCurve->abPwmNeg[ 0] = 0;
   }

   //
   // convert acceleration and deceleration to counts per control period
   // and multiply by the exactly same fudge factor ACCEL_DECEL_FACTOR
   // that is used in control.c:ramp_up()
   //
   pCurve->nAccel =
      (((UINT32)pNvmCurve->bAccelIncrement * (ACCEL_DECEL_FACTOR * CONTROL_CYCLE))
       / pNvmCurve->bPeriod);

   pCurve->nDecel =
      (((UINT32)pNvmCurve->bDecelIncrement * (ACCEL_DECEL_FACTOR * CONTROL_CYCLE))
       / pNvmCurve->bPeriod);
}



